# -- coding: utf-8 --
"""
# [Example] Miahx© 交互框架信息表示模型的 DDS 消息收发示例

**模块：MIAHX© / 框架基础服务层（FFST） / 交互框架信息表示模型（IFIRM）**

Version: 0.4.10,
Last updated: October 13, 2024

## Contributor(s)
* YAO Naiming <[naiming@iscas.ac.cn](mailto:naiming@iscas.ac.cn)>

## Change log
### v0.4.10
- .13: [更新] 适配新消息定义
- .11: [更新] 重命名 Gaze-* (deprecated)
- .9: [更新] packed 模式支持 timestamps 和 sns 字段 (deprecated)
- .2: [更新] 适配新接口 (deprecated)
### v0.4.7
- .24：[新增] 初始实现

---
Copyright © 2024-2025 MIAHX Contributors, ISCAS, and ASIC. All Rights Reserved.
Licensed under Apache-2.0.
"""

import threading
import time

from miahx_support.dds.fastdds import *
from miahx_support.stag import STag

import launcher


def gaze_writer(p: Participant, e: threading.Event):
    stag = STag(**dict(sensor_channel=1, sensor_domain='HUMAN', sensor_type='PVGC_SMART_EYE',
                       sensor_channel_type='EYES', sensor_modality='VEC', sensor_data_format='CSV')).encode()
    writer = GazeStreamWriter(p, stag=stag, srate=60,
                              schns=['head_position', 'head_heading', 'head_pitch', 'head_roll', 'gaze_origin_position',
                                     'gaze_direction', 'eyelid_openness'])
    while not e.is_set():
        items = [(1, 1.0, dict(head_position=[0, 1, 2], head_heading=3, head_pitch=4, head_roll=5,
                               gaze_origin_position=[0, 1, 2], gaze_direction=[1, 2, 3], eyelid_openness=0)),
                 (2, 2.0, dict(head_position=[2, 3, 4], head_heading=5, head_pitch=6, head_roll=7,
                               gaze_origin_position=[8, 9, 10], gaze_direction=[11, 12, 13], eyelid_openness=14))]
        writer.write(items, iid=0, uts=0)
        time.sleep(1)


def gaze_reader(p: Participant, e: threading.Event):
    def on_data_available(data):
        hdr = data.header()
        dm = hdr.header().domain()
        src = hdr.header().source()
        prefix = f'[{dm}::{src}::{name}]'

        schns = tuple(hdr.schns())
        samples = list(data.samples())
        log.info(f'{prefix} stag = `{hdr.stag()}`')
        log.info(f'{prefix} srate = `{hdr.srate()}`')
        log.info(f'{prefix} schns = `{schns}`')
        log.info(f'{prefix} elements = [{len(samples)}]')

        for i, sample in enumerate(samples):
            log.info(f'{prefix} Element [{i}]:')
            g_pos = sample.gaze_origin_position()
            g_dir = sample.gaze_direction()
            h_pos = sample.head_position()
            h_dir = [sample.head_heading(), sample.head_pitch(), sample.head_roll()]
            openness = sample.eyelid_openness()
            log.info(f'{prefix} \t\teye openness = {openness}')
            log.info(f'{prefix} \t\tgaze = {g_pos[0], g_pos[1], g_pos[2]}, {g_dir[0], g_dir[1], g_dir[2]}')
            log.info(f'{prefix} \t\theadpose = {h_pos[0], h_pos[1], h_pos[2]}, {h_dir[0], h_dir[1], h_dir[2]}')
            log.info(f'{prefix} End Element [{i}] --------')

        log.info(f'{prefix} ----------------')

    with GazeStreamReader(p, on_data_listeners=[on_data_available]) as reader:
        name = type(reader).__name__
        log.success(f'[{name}] Topic: {reader.topic_name}')
        reader.wait(e)


if __name__ == '__main__':
    log = launcher.log
    event = threading.Event()
    participant = Participant()

    launcher.run(lambda: (event, [
        threading.Thread(target=gaze_writer, args=(participant, event), daemon=False),
        threading.Thread(target=gaze_reader, args=(participant, event), daemon=False),
    ]))
